#include <algorithm>
#include <iostream>
#include <thread>

#include <ros/ros.h>
#include "std_msgs/String.h"
#include "include/System.h"
#include "mnorbslam_msgs/mnorbslam.h"



const std::string sUri = "mongodb://localhost:27017";
ORB_SLAM2::Map *mpMap;
ORB_SLAM2::LocalMapping* mpLocalMapper;
ORB_SLAM2::ORBVocabulary* mpVocabulary;
ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase;
mongocxx::instance mInstance;

bool Start_flag = 0;

void InsertFrame(const std_msgs::String& _str)
{
    mpLocalMapper->InsertKeyFrame(_str.data);
    Start_flag++;
}

void UpdatetoMap(const mnorbslam_msgs::mnorbslam &tmsg)
{
    std::pair<std::string, std::set<std::string> > mKFBuffer;
    std::pair<std::string, std::set<std::string> > mMPBuffer;

    if (tmsg.var_type=="Keyframe")
    {
        mKFBuffer.first = tmsg.var_id;
        mKFBuffer.second.insert(tmsg.var_sets.begin(), tmsg.var_sets.end());

        mpLocalMapper->Update_KFtoMap(mKFBuffer);

    }
    else if(tmsg.var_type=="MapPoint")
    {
        mMPBuffer.first = tmsg.var_id;
        mMPBuffer.second.insert(tmsg.var_sets.begin(), tmsg.var_sets.end());

        mpLocalMapper->Update_MPtoMap(mMPBuffer);
    }

}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "LocalMapping");
    ros::NodeHandle n("~");

    if (argc!=2)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_Vocabulary" << endl;
        ros::shutdown();
        return 1;
    }

    string strVocFile;
    strVocFile = string(argv[1]);

    //! Database

    // Buyi init the service params
    bool tLMAcceptFrames, tLMAbortBA, tLMNotStop, tLMStopRequested, tLMResetRequested, tLMFinishRequested, tLMStopped;
    int tLMNewFramesSize;
    n.param<bool>("mLMAcceptFrames", tLMAcceptFrames, false);
    n.param<bool>("mLMAbortBA", tLMAbortBA, false);
    n.param<int>("mLMNewFramesSize", tLMNewFramesSize, 0);
    n.param<bool>("tLMNotStop", tLMNotStop, 0);
    n.param<bool>("tLMStopRequested", tLMStopRequested, false);
    n.param<bool>("tLMResetRequested", tLMResetRequested, false);
    n.param<bool>("tLMFinishRequested", tLMFinishRequested, false);
    n.param<bool>("tLMStopped", tLMStopped, false);

    ros::Subscriber mTktoLM_subscriber = n.subscribe("/Tracking/FrametoLocalMapping", 100, InsertFrame);
    ros::Subscriber mLMupdate_subscriber = n.subscribe("/Tracking/TkUpdatemsgs", 100, UpdatetoMap);

    mpVocabulary = new ORB_SLAM2::ORBVocabulary();
    // TODO
    bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        exit(-1);
    }
    //Create KeyFrame Databases
    mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary);
    mpMap = new ORB_SLAM2::Map(mpKeyFrameDatabase, sUri);

    mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, ORB_SLAM2::System::RGBD, &n);
    std::thread mptLocalMapping = thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
    std::thread processUpdate(&ORB_SLAM2::LocalMapping::Update_NewtoDatabase, mpLocalMapper);
    ros::spin();

    return 0;
}
